#include "cmsis_os.h"
#include "pid.h"
#include "arm_math.h"
#include "CAN_receive.h"
#include "INS_task.h"
#include "main.h"
#include "remote_control.h"
#include "nucCommu.h"
#include "naive_gimbal_task.h"
#include "robot_total_mode.h"
#include "usb_task.h"
#include "OLED.h"
#include "user_lib.h"
#include "detect_task.h"
#include "RobotStructure.h"

#ifdef DOWN_GIMBAL

#define GIMBAL_TASK_INIT_TIME 2000
#define GIMBAL_TASK_CTRL_TIME 2

// 通过ST studio调试PID参数
// #define FINDING_PID

// #define ZERO_CURRENT_SAFE

#define SAFE_AUTO_SHIFT_TIME 1500   //自动模式时，当装甲板丢失时，使用手动控制，
                //但不清空操作手鼠标偏移量。当重新从手动进入自动模式模式时，操作手可能因为来不及反应
                //导致偏移量破坏。因此设计安全时间，当重新进入时，有一段时间无视操作手新增偏移量。
                //这段时间为SAFE_AUTO_SHIFT_TIME 单位ms


//****************************机器人测试获得值*************//

#define PITCH_TEST_RAD_MIN (-0.63f)
#define PITCH_TEST_RAD_MAX  (0.08f)


// 输入角速度 rad/s 、输出电压 int16_t 的PID系数
#define PITCH_SPD_KP 1590.0f
#define PITCH_SPD_KI 0.0f
#define PITCH_SPD_KD 0.0f

#define PITCH_VOLT_MAX_OUT  30000.0f
#define PITCH_VOLT_MAX_IOUT 5000.0f


#define PITCH_AGL_KP 100.0f
#define PITCH_AGL_KI 0.0f
#define PITCH_AGL_KD 0.0f

#define PITCH_AGL_SPD_MAX_OUT (30.0f)
#define PITCH_AGL_SPD_MAX_IOUT (0.7f)

#define YAW_SPD_KP 800.0f
#define YAW_SPD_KI 0000.0f
#define YAW_SPD_KD 000.0f

#define YAW_VOLT_MAX_OUT  30000.0f
#define YAW_VOLT_MAX_IOUT 5000.0f


//输入角度 rad ，输出角速度rad/s 的PID系数
#define YAW_AGL_KP 30.0f
#define YAW_AGL_KI 0.0f
#define YAW_AGL_KD 0.0f

#define YAW_AGL_SPD_MAX_OUT (50.0f)
#define YAW_AGL_SPD_MAX_IOUT (0.7f)


//一阶滤波器的参数,专门给。这两个值越大，理论上越丝滑
#define PITCH_SPD_FILTER_NUM 15.0f
#define YAW_SPD_FILTER_NUM 15.0f

//*****************************遥控器通道选项**************//
#define GIMBAL_YAW_OR_CHASSIS_W_CHANNEL 0     //右边的左右摇杆
#define SWITCH_SA 5          //遥控器两档乒乓开关 SA
#define GIMBAL_PITCH_CHANNEL 1  //右边的前后摇杆
#define RC_DEADLINE 10  //低于这个值的摇杆读数一律为0
#define TO_CHASSIS_COEF 1

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }


//****************************控制量计算系数**************//
// 使用遥控器和键盘控制云台，被控制量为速度
// 遥控器和键盘控制都是速度，控制量和速度
// NUC控制时，目标控制量为角度
#define YAW_RC_SEN      0.000004f 
#define PITCH_RC_SEN    -0.000005f //0.005
#define YAW_MOUSE_SEN   -0.00005f
#define PITCH_MOUSE_SEN -0.00005f

#define virtual_YAW_RC  660
//估计数量级

// 遥控器最大值660
// 0.5秒(500ms)角度增量 1.7rad
//5.1515151515151515151515151515152e-6

//鼠标最大值：32767
//0.5秒(500ms)角度增量 1.7rad
// 1.0376293221839045381023590807825e-7

//限幅函数 //test done
#define range_limit_inside(toBeLimited, range)                      \
    {                                                               \
         if((toBeLimited)>(range))                                  \
        {                                                           \
            (toBeLimited)=(range);                                  \
        }                                                           \
        else if((toBeLimited)<-(range))                             \
        {                                                           \
            (toBeLimited)=-(range);                                 \
        }                                                           \
    }

//这个里面的东西要占内存，不用的注释掉【指上云台】
struct gimbalMotorCtrl_s{
    //rads
    const fp32 * anglePoint;          //从姿态解算任务得到的角度位置
    const fp32 * gyroPointer;          //陀螺仪角速度指针
    fp32 nowAbsoluteAngle;      //当前姿态角
    fp32 radSpeed;              //角速度 rad/s
    fp32 wantedAbsoluteAngle;   //目标姿态角

    //PIDs
    pid_type_def spd_pid;       //用于控制速度的PID环。输入角速度 rad/s, 输出GM6020电压 int16
    pid_type_def agl_pid;       //用于控制角度的PID环。输入角度 rad,输出角速度rad/s

    //控制量
    int16_t giveVolt;       //GM6020发送电压
    uint8_t zeroVoltMark;

    // 一阶滤波器，过滤角速度
    first_order_filter_type_t spd_filter;
};


static struct gimbalMotorCtrl_s gimbalPitchCtrl,gimbalYawCtrl;
static struct gimbalMotorCtrl_s *gimbalCtrl[2]={&gimbalPitchCtrl,&gimbalYawCtrl};

#ifdef FINDING_PID
fp32 yawSpdKp = YAW_SPD_KP, yawSpdKi = YAW_SPD_KI, yawSpdKd = YAW_SPD_KD
    , pitchSpdKp = PITCH_SPD_KP, pitchSpdKi = PITCH_SPD_KI, pitchSpdKd = PITCH_SPD_KD;
    
fp32 yawAglKp = YAW_AGL_KP, yawAglKi = YAW_AGL_KI, yawAglKd = YAW_AGL_KD
    , pitchAglKp = PITCH_AGL_KP, pitchAglKi = PITCH_AGL_KI, pitchAglKd = PITCH_AGL_KD;
#endif

//这个里面的东西要占内存，不用的注释掉
static fp32 yawSpdPIDco[3]={YAW_SPD_KP,YAW_SPD_KI,YAW_SPD_KD};
static fp32 pitchSpdPIDco[3]={PITCH_SPD_KP,PITCH_SPD_KI,PITCH_SPD_KD};
static fp32 yawAglPIDco[3]={YAW_AGL_KP,YAW_AGL_KI,YAW_AGL_KD};
static fp32 pitchAglPIDco[3]={PITCH_AGL_KP,PITCH_AGL_KI,PITCH_AGL_KD};

static const enum RobotState_e *robotMode;
//控制指针
static const RC_ctrl_t *rc_p;   //遥控器位置指针，需要初始化
static const toSTM32_t *nuc_p;  //NUC数据位置。未来制造全自动机器人时需要

// 抄一个过来
// https://blog.csdn.net/CSDN_X_W/article/details/90289021

//1. 结构体类型定义
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.00001
    float R;//观测噪声协方差 初始化值为0.00003
}KFP;//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
static KFP KFP_height={0.02,0,0,0.1,0.0001,0.0003};

static KFP KFP_pitch_agl={0.02,0,0,0.05,0.0001,0.0003};

static KFP KFP_pitch_wantedagl={0.02,0,0,0.9,0.0001,0.0003};
static KFP KFP_yaw_wantedagl={0.02,0,0,0.9,0.0001,0.0003};

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
 static float kalmanFilter(KFP *kfp,float input)
 {
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    //  kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);

     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out;
 }

/**
 *调用卡尔曼滤波器 实践
 */
// int height;
// int kalman_height=0;
// kalman_height = kalmanFilter(&KFP_height,(float)height);,


static void initGimbalCtrls(void)
{
    uint8_t i;
    //初始化绝对角度位置
    gimbalYawCtrl.anglePoint=get_INS_angle_point();         //第一个角度
    gimbalPitchCtrl.anglePoint=(&(get_INS_angle_point()[2])); //第三个角度
    gimbalYawCtrl.gyroPointer=&(get_gyro_data_point()[2]);
    gimbalPitchCtrl.gyroPointer=get_gyro_data_point();



    // 初始化PID参数
    // 两个双环PID不共用参数
    PID_init(&(gimbalPitchCtrl.spd_pid),PID_POSITION,pitchSpdPIDco,PITCH_VOLT_MAX_OUT,PITCH_VOLT_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.spd_pid),PID_POSITION,yawSpdPIDco,YAW_VOLT_MAX_OUT,YAW_VOLT_MAX_IOUT);
    
    PID_init(&(gimbalPitchCtrl.agl_pid),PID_POSITION,pitchAglPIDco,PITCH_AGL_SPD_MAX_OUT,PITCH_AGL_SPD_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.agl_pid),PID_POSITION,yawAglPIDco,YAW_AGL_SPD_MAX_OUT,YAW_AGL_SPD_MAX_IOUT);

    //滤波初始化
    const static fp32 gimbal_pitch_order_filter[1] = {PITCH_SPD_FILTER_NUM};
    const static fp32 gimbal_yaw_order_filter[1] = {YAW_SPD_FILTER_NUM};

    first_order_filter_init(&(gimbalPitchCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_pitch_order_filter);
    first_order_filter_init(&(gimbalYawCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_yaw_order_filter);

    
    //初始化当前角度和目标角度、pid参数、里程碑栈法数据
    for(i=0;i<2;i++)
    {
        (gimbalCtrl[i])->zeroVoltMark=0;
        (gimbalCtrl[i])->nowAbsoluteAngle=*((gimbalCtrl[i])->anglePoint);
        gimbalCtrl[i]->wantedAbsoluteAngle=(gimbalCtrl[i])->nowAbsoluteAngle;
    }
    PID_clear(&gimbalYawCtrl.agl_pid);
    PID_clear(&gimbalYawCtrl.spd_pid);
    PID_clear(&gimbalPitchCtrl.agl_pid);
    PID_clear(&gimbalPitchCtrl.spd_pid);
    gimbalYawCtrl.radSpeed = 0;
    gimbalPitchCtrl.radSpeed = 0;
    
}

/**
 * @brief 将角度化为(-PI,PI)范围内
 * 
 * @param rawAngle 
 * @return fp32 
 */
static fp32 radFormat(fp32 rawAngle)   //test done
{
    while(rawAngle>PI)
        rawAngle-=(2*PI);
    while(rawAngle<(-PI))
        rawAngle+=(2*PI);
    return rawAngle;
}

/**
 * @brief 计算转圈的PID。内部error先循环转化为(-PI,PI)范围
 * 
 * @param pid 
 * @param ref 反馈值
 * @param set 设定值
 * @return fp32 
 */
static fp32 gimbal_PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = radFormat(set - ref);
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        range_limit_inside(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    return pid->out;
}




static void refreshAngleStates(void)
{
    //当前姿态角和角速度
    gimbalYawCtrl.nowAbsoluteAngle  =   *(gimbalYawCtrl.anglePoint);
    gimbalYawCtrl.radSpeed          =   *(gimbalYawCtrl.gyroPointer);
    gimbalPitchCtrl.nowAbsoluteAngle=   *(gimbalPitchCtrl.anglePoint);
    gimbalPitchCtrl.radSpeed        =   *(gimbalPitchCtrl.gyroPointer);
}

/**
 * @brief 根据机器人状态控制pitch和yaw 旋转方向或者传给底盘的速度
 * 
 */
static void getControlAngles(void)
{
    static int16_t yaw_channel = 0, pitch_channel = 0;
    rc_deadband_limit(rc_p->rc.ch[GIMBAL_YAW_OR_CHASSIS_W_CHANNEL], yaw_channel, RC_DEADLINE);
    yaw_channel=-yaw_channel;
    rc_deadband_limit(rc_p->rc.ch[GIMBAL_PITCH_CHANNEL], pitch_channel, RC_DEADLINE);
    static enum RobotState_e lastMode=RobotState_e_Powerless;   //初始化为无力
    if(lastMode!=(*robotMode))
        gimbalYawCtrl.wantedAbsoluteAngle=gimbalYawCtrl.nowAbsoluteAngle;
    lastMode=*robotMode;
    //根据模式选择标志
    gimbalYawCtrl.zeroVoltMark=0;
    gimbalPitchCtrl.zeroVoltMark=0;

    if(*robotMode==RobotState_e_Powerless)
    {
        gimbalYawCtrl.zeroVoltMark=1;
        gimbalPitchCtrl.zeroVoltMark=1;

    }    
    else if(*robotMode==RobotState_e_Brilliant)
    {
        if(gimbal_NUC_is_available())
        {
            gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + nuc_p->yawDown; 
            gimbalPitchCtrl.wantedAbsoluteAngle = gimbalPitchCtrl.nowAbsoluteAngle + nuc_p->pitchDown; 
            downgimbalClearReadMark();
        }
    }
    else
    {   
        if(switch_is_down(rc_p->rc.ch[SWITCH_SA])){
            gimbalYawCtrl.wantedAbsoluteAngle += GIMBAL_TASK_CTRL_TIME*yaw_channel * YAW_RC_SEN;
            gimbalPitchCtrl.wantedAbsoluteAngle += GIMBAL_TASK_CTRL_TIME*pitch_channel * PITCH_RC_SEN;
        }else{
            gimbalPitchCtrl.wantedAbsoluteAngle=0;
        }
    }

    gimbalYawCtrl.wantedAbsoluteAngle = kalmanFilter(&KFP_yaw_wantedagl,gimbalYawCtrl.wantedAbsoluteAngle);
    gimbalPitchCtrl.wantedAbsoluteAngle = kalmanFilter(&KFP_pitch_wantedagl,gimbalPitchCtrl.wantedAbsoluteAngle);
}

// 限制幅度在
static void limitAngles()
{
    // 超过最大2PI角度时,限制幅度
    struct gimbalMotorCtrl_s * c;
    int i;
    for(i = 0 ; i < 2; i++)
    {
        c = gimbalCtrl[i];
        if(radFormat(c->wantedAbsoluteAngle - c ->nowAbsoluteAngle) > (PI/2))
            c->wantedAbsoluteAngle = c ->nowAbsoluteAngle + (PI/2);
        if(radFormat(c->wantedAbsoluteAngle - c ->nowAbsoluteAngle) < (-PI/2))
            c->wantedAbsoluteAngle = c ->nowAbsoluteAngle - (PI/2);
    }

    // 限制pitch轴绝对角度。考虑到当前机器人在平地上运动，则全用rad控制。
    if(radFormat(gimbalPitchCtrl.wantedAbsoluteAngle)>PITCH_TEST_RAD_MAX)
        gimbalPitchCtrl.wantedAbsoluteAngle=PITCH_TEST_RAD_MAX;
    if(radFormat(gimbalPitchCtrl.wantedAbsoluteAngle)<PITCH_TEST_RAD_MIN)
        gimbalPitchCtrl.wantedAbsoluteAngle=PITCH_TEST_RAD_MIN;

    // gimbalYawCtrl.wantedAbsoluteAngle = radFormat(gimbalYawCtrl.wantedAbsoluteAngle);
    // gimbalPitchCtrl.wantedAbsoluteAngle = radFormat(gimbalPitchCtrl.wantedAbsoluteAngle);
    
}

static void calcPID(void)
{
    uint8_t i;
    struct gimbalMotorCtrl_s * c;
    for(i=0;i<2;i++)
    {
        c=gimbalCtrl[i];

        gimbal_PID_calc(&(c->agl_pid),c->nowAbsoluteAngle,c->wantedAbsoluteAngle);  // 关乎旋转方向的PID控制器
        // 输出了所需旋转速度。

        //对速度进行一阶滤波
        // first_order_filter_cali(&(c->spd_filter),(c->agl_pid).out);

        PID_calc(&(c->spd_pid),c->radSpeed,(c->agl_pid).out);   // 普通的速度控制环
        c->giveVolt=c->spd_pid.out;     //给电压

        if(c->zeroVoltMark==1)
        {
            c->giveVolt=0;
        }
    }

}

static void voltchange(void)
{
    gimbalPitchCtrl.giveVolt=-gimbalPitchCtrl.giveVolt;
}

void down_gimbal_task(void const *pvParameters)
{
    initGimbalCtrls();
    osDelay(GIMBAL_TASK_INIT_TIME);
    robotMode=getRobotPresentMode();
    rc_p=get_remote_control_point();    //获取遥控器数据和NUC数据指针
    nuc_p=get_nuc_control_point();

    #ifdef UP_GIMBAL
    int16_t *yawupCurrentP=getYawUpCurrentP();//获取Upgimbal源文件里计算的yawup的电流。
    int16_t *pitchupCurrentP=getPitchUpCurrentP();
    #endif
    while(1)
    {
            refreshAngleStates();   // 从传感器获得当前角度数据 //test done
            getControlAngles();                  // 从遥控器获得目标角度     //test half done
            limitAngles();
            calcPID();              // 总是控制角度，双环，但测试时需要
            voltchange();
            #ifdef ZERO_CURRENT_SAFE
            CAN_cmd_downgimbal(0,0);
            CAN_cmd_GM_and_downSTM(0,0);
            #else
            #ifdef UP_GIMBAL
            CAN_cmd_downgimbal(*yawupCurrentP,gimbalPitchCtrl.giveVolt);
            CAN_cmd_GM_and_downSTM(gimbalYawCtrl.giveVolt,*pitchupCurrentP);
            #else
            CAN_cmd_downgimbal(0,0);
            CAN_cmd_GM_and_downSTM(gimbalYawCtrl.giveVolt,0);
            #endif
            #endif
        osDelay(GIMBAL_TASK_CTRL_TIME);
    }
}

fp32 getDownGimbalYaw(void)
{
    return gimbalYawCtrl.nowAbsoluteAngle;
}

static void OLED_gimbal(void)
{
    // 注意，写得太长可能显示不出来
    // OLED_printf(0,0,"agl:%.2f, spd:%.2f", gimbalPitchCtrl.nowAbsoluteAngle, gimbalPitchCtrl.radSpeed);
    // OLED_printf(0,0,"agl:%.2f, spd:%.2f", gimbalYawCtrl.nowAbsoluteAngle, gimbalYawCtrl.radSpeed);
    // OLED_printf(0,0,"acc:%.4f | %.4f | %.4f", get_accel_data_point()[0], 
    //     get_accel_data_point()[1], get_accel_data_point()[2] );
    // OLED_printf(0,0,"P:AGL:%.4f,SPD:%.4f,k_kg:%.4f",
        // gimbalPitchCtrl.nowAbsoluteAngle,gimbalPitchCtrl.radSpeed,KFP_height.Kg);
}

#endif
